#include <DATASCOPE.h>

DATASCOPE scope;
#define TARGET  1000

int STBY = 6;       /* STBY为正常工作/待机状态控制引脚 */

int AIN1 = 7;
int AIN2 = 8;
int PWMA = 10;

int ENCODEA = 3;    /* 电机测速编码器计数引脚 */
int ENCODEB = 11;   /* 电机测速编码器计数引脚 */

int count = 0;      /* 中断计数值 */
float t = 0;        /* 系统启动时间 */
float v = 0;        /* 计算得到的平均转速, 单位 r/min */

int target = TARGET;
int loop_cnt = 0;

/* 位置 PID 相关变量 */
float bias_last         = 0;    /* 上一次偏差量 */
float bias_current      = 0;    /* 当前偏差量 */
float bias_integral     = 0;    /* 偏差积分值 */
float bias_differential = 0;    /* 偏差微分值 */

float Kp = 100;
float Ki = 0;
float Kd = 1;

void setup()
{
  /* 设置引脚均为输出模式 */
  pinMode(STBY, OUTPUT);

  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);

  /* 设置 PIN3 为外部中断引脚, 中断触发方式为双边沿触发 */
  attachInterrupt(digitalPinToInterrupt(ENCODEA), PIN3ISR, RISING);

  /* 设置 TB6112 为正常工作模式 */
  digitalWrite(STBY, HIGH);

  /* 设置电机正转 */
  digitalWrite(AIN1, HIGH);
  digitalWrite(AIN2, LOW);
  analogWrite(PWMA, 0);

  scope.baudrate(128000);

  delay(3000);
}

int pid_position(int encode, int target);

void loop()
{
  int pwm = 0;
  loop_cnt++;

  pwm = pid_position(count, target);
  if (pwm > 0)
  {
    digitalWrite(AIN1, LOW);
    digitalWrite(AIN2, HIGH);
  }
  else
  {
    pwm = 0 - pwm;
    digitalWrite(AIN1, HIGH);
    digitalWrite(AIN2, LOW);
  }
  analogWrite(PWMA, pwm);

  scope.update(1, count);
  scope.put(1);

  delay(5);

  if (loop_cnt >= 800)
  {
    loop_cnt = 0;
    /* 5ms * 800 = 4s */
    target = target + TARGET;
  }
}

void PIN3ISR(void)
{
  if (0 == digitalRead(ENCODEB))
  {
    count++;
  }
  else if (1 == digitalRead(ENCODEB))
  {
    count++;
  }
}

int pid_position(int encode, int target)
{
  bias_current = encode - target;
  bias_integral = bias_current + bias_integral;
  bias_differential = bias_current - bias_last;
  bias_last = bias_current;
  return Kp * bias_current + Ki * bias_integral + Kd * bias_differential;
}
